//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcMoveTask.h>
#include <rc_task/rcTaskManager_DataStruct.h>
#include <rc_move/rcmove.h>
#include <base/slog.hpp>
#include <iostream>
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <rc_message/rc_msg_server.h>
#include <rc_task/queue_sources_locks.h>
#include <vector>
namespace RC {
    namespace Task {

        void run_move_task(rc_MoveDevice rcMoveDevice) {
            using namespace RC::Message;
            slog::info << "制动任务启动中" << slog::endl;
            RobotCarMove robot;
            robot.init(rcMoveDevice);
            try {
                while (true) {
                    boost::this_thread::interruption_point();
                    boost::mutex::scoped_lock lock(MoveMessage::move_mutex);
                    if (MessageServer::moveMessage.size_message() != 0) {
                        WHEEL_DATA wheelData = MessageServer::moveMessage.front_message();
                        robot.excute(wheelData);
                        // TODO:这个POP要不要存在争议
                        MessageServer::moveMessage.pop_message();
                    }
                }
            }catch (boost::thread_interrupted &e){
                slog::info << "运动控制任务关闭" << slog::endl;
//                robot.wheel_stop();
            }
        }
    }
}
